package ro.ubbcluj.android.PhysiTrack;

import android.graphics.Bitmap;
import android.os.Message;

public class Calculate implements Runnable {

	private PhysiTrackActivity phisiTrack;
	private Thread control = null;
	private boolean isCalculating = false;
	private UIHandler handler;
	private double[][] data;
	private int db;
	Bitmap image;
	//kalman filter
	private double[][] C;
	private double sigmaW;
	private double sigmaM;
	
	public void setSigmaW(double w){
		sigmaW = w;
	}
	
	
	public Calculate(PhysiTrackActivity phisiTrack, UIHandler handler) {
		this.phisiTrack = phisiTrack;
		this.handler = handler;
		this.data = new double[10000][7];
		this.C = new double [3][3];
		sigmaW = 0.1;
		sigmaM = 0.1;
		for(int i=0;i<3;i++)
			for(int j=0;j<3;j++)
				C[i][j]=0.1;
	}

	@Override
	public void run() {
		
		// atalakit
		
		while (isCalculating && db < 10000) {
			try {
				Thread.sleep(500);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
			// /beallit seb+gyors: pos[0]+pos[1]
			//Message msg = Message.obtain(handler);
			//msg.obj = Math.sqrt(data[db][3]*data[db][3]+data[db][4]*data[db][4]) + " " + Math.sqrt(data[db][5]*data[db][5]+data[db][6]*data[db][6]);
			
			//handler.sendMessage(msg);
			
			
			
		}

	}

	public void startCalculate() {
		control = new Thread(this);
		isCalculating = true;
		control.start();

	}

	public void stopCalculate() {
		if (control != null) {
			isCalculating = false;
			Thread tmp = control;
			tmp.interrupt();

		}
	}

	public synchronized void setData(int px, int py){
		
		if(db>0)
			{			
			//kalman filter ide
			double time = 0.2;//deltaT
			//1predict
			double xPosPredict = data[db][1] + time * data[db][3] + time*time/2 * data[db][5]; 
			double yPosPredict = data[db][2] + time * data[db][4] + time*time/2 * data[db][6];
			double xVelPredict = data[db][3] + time * data[db][5];
			double yVelPredict = data[db][4] + time * data[db][6];
			double xAccPredict = data[db][5];
			double yAccPredict = data[db][6];
			double[][] CPredict = new double[3][3];
			CPredict[0][0] = C[0][0] + time*C[1][0] + 1/2*time*time*C[2][0] + time*(C[0][1]+time*C[1][1]+time*time*1/2*C[2][1]) + time*time*1/2*(C[0][2]+time*C[1][2]+time*time*1/2*C[2][2]) + sigmaW;
			CPredict[0][1] = C[0][1] + time*C[1][1] + time*time/2 * C[2][1] + time*(C[0][2]+time*C[1][2]+time*time/2*C[2][2]);
			CPredict[0][2] = C[0][2] + time*C[1][2] + time*time/2 * C[2][2];
			CPredict[1][0] = C[1][0] + time*C[2][0] + time * (C[1][1] + time*C[2][1]) + time*time/2*(C[1][2]+time*C[2][2]);
			CPredict[1][1] = C[1][1] + time*C[2][1] + time * (C[1][2] + time*C[2][2]) + sigmaW;
			CPredict[1][2] = C[1][2] + time*C[2][2];
			CPredict[2][0] = C[2][0] + time*C[2][1] + time*time/2*C[2][2];
			CPredict[2][1] = C[2][1] + time*C[2][2];
			CPredict[2][2] = C[2][2] + sigmaW;
			//2update
			double diffPosX = px - (data[db][1] + time*data[db][3] + time*time/2*data[db][5]);
			double diffPosY = py - (data[db][2] + time*data[db][4] + time*time/2*data[db][6]);
			double S = CPredict[0][0] + sigmaM;//kerdes, hogy vane ket S, sztem nincs
			double[] K = new double[3];
			K[0] = CPredict[0][0]/S;
			K[1] = CPredict[1][0]/S;
			K[2] = CPredict[2][0]/S;
			//3seting
			data[db][1]=xPosPredict + K[0]*diffPosX;			
			data[db][2]=yPosPredict + K[0]*diffPosY;
			data[db][3]=xVelPredict + K[0]*diffPosX/time;
			data[db][4]=xVelPredict + K[0]*diffPosY/time;
			data[db][5]=xAccPredict + K[0]*diffPosX/time*time;
			data[db][6]=yAccPredict + K[0]*diffPosY/time*time;
			
			//C megvaltoztatasa
			C[0][0] = CPredict[0][0] * (1-K[0]);
			C[0][1] = CPredict[0][1] * (1-K[0]);
			C[0][2] = CPredict[0][2] * (1-K[0]);
			C[1][0] = CPredict[0][0] * K[1]+CPredict[1][0];
			C[1][1] = CPredict[0][1] * K[1]+CPredict[1][1];
			C[1][2] = CPredict[0][2] * K[1]+CPredict[1][2];
			C[2][0] = CPredict[0][0] * K[2]+CPredict[2][0];
			C[2][1] = CPredict[0][1] * K[2]+CPredict[2][1];
			C[2][2] = CPredict[0][2] * K[2]+CPredict[2][2];			
			//adatoktovabb
			Message msg = Message.obtain(handler);
			//msg.obj = px + " " + py;
			msg.obj = Math.sqrt(data[db][3]*data[db][3]+data[db][4]*data[db][4]) + " " + Math.sqrt(data[db][5]*data[db][5]+data[db][6]*data[db][6]);
			handler.sendMessage(msg);
			db++;
			}
		else{db++;}
	}
	public synchronized void setImage(Bitmap image) {
		this.image = image;
	}

	public synchronized Bitmap getImage() {
		return image;
	}

	
	public int getDb(){
		return this.db;
	}

	public double[][] getData() {
		double[][] result = new double[10000][5];
		for(int i=0;i<db;i++){
			result[i][0]=data[i][0];
			result[i][1]=data[i][1];
			result[i][2]=data[i][2];
			result[i][3]=Math.sqrt(data[i][3]*data[i][3]+data[i][4]*data[i][4]);
			result[i][4]=Math.sqrt(data[i][5]*data[i][5]+data[i][6]*data[i][6]);
		}
		return result;
	}

}
